#ifndef SHERIFFOS_DJI_MOTOR_H
#define SHERIFFOS_DJI_MOTOR_H

#include <connectivity/can.h>
#include <clock.h>
#include <connection_guard.h>
#include <units.h>

enum class DJIMotorState : uint8_t { kConnected = 0, kLost };

struct Dji6020V {
    static constexpr float angleConvert_ = 8192.f / 360;   ///< 角度转换比例
    static constexpr float currentConvert_ = 16384.f / 3;  ///< 电流转换比例
};

struct Dji6020I {
    static constexpr float angleConvert_ = 8192.f / 360;   ///< 角度转换比例
    static constexpr float currentConvert_ = 16384.f / 3;  ///< 电流转换比例
};

struct Dji3508 {
    static constexpr float angleConvert_ = 8192.f / 360;    ///< 角度转换比例
    static constexpr float currentConvert_ = 16384.f / 20;  ///< 电流转换比例
};

struct Dji2006 {
    static constexpr float angleConvert_ = 8192.f / 360;    ///< 角度转换比例
    static constexpr float currentConvert_ = 10000.f / 10;  ///< 电流转换比例
};

struct DjiEncoderData {
    units::angle::degree_t rawAngle{};              ///< 编码器原始角度
    units::angle::degree_t limitedAngle{};          ///< 偏移后角度，归一化到-180-180
    units::angle::degree_t lastLimitedAngle{};      ///< 归一化角度记忆
    int32_t roundCnt{};                             ///< 连续角圈数计数
    units::angle::degree_t shaftContinuousAngle{};  ///< 输出轴连续角度(含角度偏移)

    units::angular_velocity::revolutions_per_minute_t rawSpeed{};    ///< 编码器原始速度
    units::angular_velocity::revolutions_per_minute_t shaftSpeed{};  ///< 输出轴速度

    units::current::ampere_t current{};  ///< 电流

    float temperature{};  ///< (单位：摄氏度)温度
};

template <typename MotorTag>
class DjiEncoder {
    DjiEncoderData data_;
    units::time::second_t lastUpdateTime_{};

    units::angle::degree_t angleOffset_{};  ///< 编码器角度偏置
    float gearRatio_{};                     ///< 减速比

public:
    DjiEncoder() = default;

    /**
     * @param gearRatio 减速比
     * @param angleOffset 角度偏置
     */
    explicit DjiEncoder(float const gearRatio, units::angle::degree_t const angleOffset)
        : angleOffset_(angleOffset), gearRatio_(gearRatio) {
        os_assert(gearRatio != 0, "");
    }

    ~DjiEncoder() = default;

    void decode(uint8_t const* data) {
        data_.lastLimitedAngle = data_.limitedAngle;
        lastUpdateTime_ = os::getTime();

        data_.rawAngle = units::make_unit<units::angle::degree_t>(static_cast<uint16_t>(data[0]) << 8 |
                                                                  static_cast<uint16_t>(data[1])) /
                         MotorTag::angleConvert_;

        float tmpAngle = data_.rawAngle.to<float>() - angleOffset_.to<float>();

        while (tmpAngle > 180) tmpAngle -= 360;
        while (tmpAngle <= -180) tmpAngle += 360;

        data_.limitedAngle = units::make_unit<units::angle::degree_t>(tmpAngle);
        if (data_.limitedAngle - data_.lastLimitedAngle < -180_deg) {
            data_.roundCnt++;
        } else if (data_.limitedAngle - data_.lastLimitedAngle > 180_deg) {
            data_.roundCnt--;
        }

        data_.shaftContinuousAngle = (data_.limitedAngle + data_.roundCnt * 360.0_deg) / gearRatio_;

        data_.rawSpeed = units::make_unit<units::angular_velocity::revolutions_per_minute_t>(
            static_cast<uint16_t>(data[2] << 8) | static_cast<uint16_t>(data[3]));

        data_.shaftSpeed = data_.rawSpeed / gearRatio_;

        data_.current = units::make_unit<units::current::ampere_t>(static_cast<int16_t>(
                            static_cast<uint16_t>(data[4]) << 8 | static_cast<uint16_t>(data[5]))) /
                        MotorTag::currentConvert_;

        data_.temperature = static_cast<uint8_t>(data[6]);
    }

    void reset() { memset(&data_, 0, sizeof(data_)); }

    /**
     * @brief 获取数据
     * @return 编码器数据
     */
    [[nodiscard]] DjiEncoderData const& getData() const { return data_; }

    /**
     * @brief 获取上次更新时间
     * @return 时间
     */
    [[nodiscard]] units::time::second_t getLastUpdateTime() const { return lastUpdateTime_; }
};

template <typename MotorTag, size_t id>
class DjiMotor final {
    static_assert(((std::is_same_v<MotorTag, Dji6020I> || std::is_same_v<MotorTag, Dji6020V>) && id >= 0x205 &&
                   id <= 0x20B) ||
                      (std::is_same_v<MotorTag, Dji3508> && id >= 0x201 && id <= 0x207) ||
                      (std::is_same_v<MotorTag, Dji2006> && id >= 0x201 && id <= 0x207),
                  "id does not fit motor type");

    DJIMotorState state_{};  ///< 电机启停状态
    int16_t output_{};       ///< 电机输出

    void checkConnectionState() {
        if (os::getTime() - encoder_.getLastUpdateTime() > 200_ms || encoder_.getLastUpdateTime() == 0_ms)
            state_ = DJIMotorState::kLost;
        else
            state_ = DJIMotorState::kConnected;
    }

public:
    ~DjiMotor() = default;

    DjiEncoder<MotorTag> encoder_;

    explicit DjiMotor(float const gearRatio, units::angle::degree_t const angleOffset)
        : encoder_(gearRatio, angleOffset) {
        state_ = DJIMotorState::kLost;  ///< 默认电机启动
        reset();
        os::ConnectionGuard::addGuard([this] { checkConnectionState(); });
    }

    /**
     * @brief 获取电机id
     */
    static constexpr size_t getId() { return id; }

    [[nodiscard]] bool isLost() const { return state_ == DJIMotorState::kLost; }

    /**
     * @brief 重置电机
     */
    void reset() {
        encoder_.reset();
        output_ = 0;
    }

    /**
     * @brief 读取电机状态
     * @return DJIMotorState
     */
    [[nodiscard]] DJIMotorState getState() const { return state_; }

    /**
     * @brief 设置电机输出
     * @param output 电机输出
     */
    void setOutput(int16_t const output) { output_ = output; }

    /**
     * @brief 读取电机输出
     * @return 电机输出
     */
    [[nodiscard]] int16_t getOutput() const { return output_; }

    /**
     * @brief 解码
     * @param buffer 数组
     */
    void decode(uint8_t const* buffer) { encoder_.decode(buffer); }
};

namespace detail {
template <typename T>
constexpr size_t getMotorId() {
    return T::getId();
}

template <typename... Motors>
constexpr std::array<size_t, sizeof...(Motors)> get_motor_ids() {
    return {getMotorId<Motors>()...};
}

template <size_t N>
constexpr bool has_duplicate_ids(std::array<size_t, N> const& ids) {
    for (size_t i = 0; i < N; ++i)
        for (size_t j = i + 1; j < N; ++j)
            if (ids[i] == ids[j]) return true;
    return false;
}

template <size_t txId>
constexpr std::pair<size_t, size_t> getIdBound() {
    if constexpr (txId == 0x1FF || txId == 0x1FE) {
        return std::make_pair(0x205, 0x208);
    } else if constexpr (txId == 0x2FF || txId == 0x2FE) {
        return std::make_pair(0x209, 0x20C);
    } else if constexpr (txId == 0x200) {
        return std::make_pair(0x201, 0x204);
    } else {
        static_assert([] { return false; }(), "txId error");
    }
    return std::make_pair(0, 0);
}

/**
 * @brief 静态检查id是否合法
 */
template <size_t N, size_t txId>
constexpr bool ids_in_range(std::array<size_t, N> const& ids) {
    for (size_t i = 0; i < N; ++i)
        if (ids[i] < getIdBound<txId>().first || ids[i] > getIdBound<txId>().second) return false;
    return true;
}
}  // namespace detail

template <size_t txID, typename... Motors>
class DjiMotorGroup {
    static constexpr size_t N = sizeof...(Motors);

    os::Can& can_;
    FDCAN_TxHeaderTypeDef txHeader_;
    std::tuple<Motors&...> motors_;

    uint8_t _data[8]{};

public:
    explicit DjiMotorGroup(os::Can& can, Motors&... motors)
        : can_(can),
          txHeader_({
              .Identifier = txID,
              .IdType = FDCAN_STANDARD_ID,
              .TxFrameType = FDCAN_DATA_FRAME,
              .DataLength = FDCAN_DLC_BYTES_8,
              .ErrorStateIndicator = FDCAN_ESI_ACTIVE,
              .BitRateSwitch = FDCAN_BRS_OFF,
              .FDFormat = FDCAN_CLASSIC_CAN,
              .TxEventFifoControl = FDCAN_NO_TX_EVENTS,
              .MessageMarker = 0,
          }),
          motors_(motors...) {
        // 编译期检查
        constexpr auto ids = detail::get_motor_ids<std::remove_reference_t<Motors>...>();
        static_assert(N >= 1 && N <= 4, "Motor group must have 1~4 motors");
        static_assert(!detail::has_duplicate_ids(ids), "Duplicate motor IDs");
        static_assert(detail::ids_in_range<N, txID>(ids), "Motor ID out of range");
    }

    /**
     * @brief 获取can端口
     */
    [[nodiscard]] os::Can& getCan() const { return can_; }

    /**
     * @brief 读取电机数据并解码
     * @param motorID 电机ID
     * @param data 原始数据
     */
    void decode(uint32_t motorID, uint8_t const* data) const {
        if (motorID < detail::getIdBound<txID>().first || motorID > detail::getIdBound<txID>().second) return;

        std::apply(
            [&]<typename... T>(T&... motor) {
                ((motorID == std::remove_reference_t<T>::getId() ? (motor.decode(data), void(), true) : false) || ...);
            },
            motors_);
    }

    /**
     * @brief 电机数据发送
     */
    void TransmitData() {
        std::ranges::fill(_data, 0);

        std::apply(
            [&](auto&... motor) {
                auto emit = [&]<typename T>(T& m) {
                    constexpr size_t id = std::remove_reference_t<T>::getId();
                    constexpr size_t offset = (id - detail::getIdBound<txID>().first) * 2;

                    int16_t const output = m.getOutput();
                    _data[offset] = output >> 8;
                    _data[offset + 1] = output & 0xFF;
                };
                (emit(motor), ...);
            },
            motors_);

        can_.transmit(txHeader_, _data);
    }
};

template <size_t txId, typename... Motors>
auto make_DjiMotorGroup(os::Can& can, Motors&... motors) {
    return DjiMotorGroup<txId, Motors...>(can, motors...);
}

#define GM6020TxID_1 0x1FF
#define GM6020TxID_2 0x2FF
#define GM6020RxID_1 0x205
#define GM6020RxID_2 0x209

#define GM6020iTxID_1 0x1FE
#define GM6020iTxID_2 0x2FE

#define M3508TxID_1 0x200
#define M3508TxID_2 0x1FF
#define M3508RxID_1 0x201
#define M3508RxID_2 0x205

#define M2006TxID_1 M3508TxID_1
#define M2006TxID_2 M3508TxID_2
#define M2006RxID_1 M3508RxID_1
#define M2006RxID_2 M3508RxID_2

#endif
